# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import PoseStamped
from tf.transformations import quaternion_from_euler
from actionlib_msgs.msg import GoalStatus
from move_base_msgs.msg import MoveBaseActionResult
from std_msgs.msg import String
from itertools import cycle
from std_msgs.msg import String

goal_points = [(3.90, -7.35, -1.86), (2.60, -2.77, 3.14),(3.90, -7.35, -1.86)]
# goal_points = [(0.393, -0.184, 0), (-1.48, 5.54, 3.14),(0.393, -0.184, 0)]
cycled_points = cycle(goal_points)


class CarRobotCommunicator:
    def __init__(self):
        rospy.init_node('multi_point_navigation')
        self.goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
        self.status_sub = rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.goal_status_callback,
                                           queue_size=10)
        self.car_status_pub = rospy.Publisher("car_statue", String, queue_size=10)
        rospy.sleep(1)  # 等待节点初始化完成
        self.send_initial_goal()

    def send_goal(self):
        goal = PoseStamped()
        goal.header.frame_id = "map"

        # 获取下一个目标点
        goal_points = next(cycled_points)

        goal.pose.position.x = goal_points[0]  # x
        goal.pose.position.y = goal_points[1]  # y/

        quaternion = quaternion_from_euler(0, 0, goal_points[2])
        goal.pose.orientation.x = quaternion[0]
        goal.pose.orientation.y = quaternion[1]
        goal.pose.orientation.z = quaternion[2]
        goal.pose.orientation.w = quaternion[3]

        self.goal_pub.publish(goal)
        print("x=", goal_points[0], " y=", goal_points[1], " theta=", goal_points[2])  # 打印目标点信息

    def goal_status_callback(self, msg):
        global n
        if msg.status.status == 3:
            # global n
            n += 1
            print("Robot reached the goal")
            # print(goal_points.index((msg.goal.target_pose.pose.position.x, msg.goal.target_pose.pose.position.y)) == 0)
            # print(goal_points.index((msg.goal.target_pose.pose.position.x, msg.goal.target_pose.pose.position.y)) == 1)
            # if goal_points.index((msg.goal.target_pose.pose.position.x, msg.goal.target_pose.pose.position.y)) == 0:
            if n % 2 == 1:
                # 到达第一点，发送抓取指令
                # if n>1:
                self.notify_arm_grasp()
                self.await_arm_grasp_confirmation()
                self.send_goal()  # 小车前往第二点
                rospy.sleep(5)

            elif n % 2 == 0:
                # 到达第二点，发送放置指令
                # if n>2:
                self.notify_arm_place()
                self.await_arm_place_confirmation()
                self.send_goal()  # 小车返回第一点
                rospy.sleep(5)

    def notify_arm_grasp(self):
        print("位置1")
        # msg = rospy.wait_for_message("arm_statue", String)
        # re_msg = msg.data

        for _ in range(20):  # 发送2s
            send_data = String()
            send_data.data = "Grasp position arrived"
            print(send_data.data)
            self.car_status_pub.publish(send_data)
            rospy.sleep(0.1)

    def await_arm_grasp_confirmation(self):
        # 此处假设小车等待机械臂的抓取成功消息
        # 实际应用中，应该监听正确的机械臂反馈话题并验证其内容
        msg = rospy.wait_for_message("arm_statue", String)
        print("消息：", msg.data)

    def notify_arm_place(self):
        for _ in range(20):  # 发送2s
            send_data = String()
            send_data.data = "Place position arrived"
            self.car_status_pub.publish(send_data)
            print(send_data.data)
            rospy.sleep(0.1)

    def await_arm_place_confirmation(self):
        # 此处假设小车等待机械臂的放置成功消息
        # 实际应用中，应该监听正确的机械臂反馈话题并验证其内容
        msg = rospy.wait_for_message("arm_statue", String)
        print("消息：", msg.data)

    def send_initial_goal(self):
        self.send_goal()
        print("sent initial goal*********")


if __name__ == '__main__':
    global n
    n = 0
    rospy.sleep(0.1)
    communicator = CarRobotCommunicator()
    rospy.spin()